##!/usr/bin/python
import sys
sys.path.insert(0, '/home/rosdell/mplab-ros-pkg/wii-balance/wii_balance/cwiid/python/build/lib.linux-x86_64-2.7/')
sys.path.insert(0, '/home/rosdell/mplab-ros-pkg/wii-balance/wii_balance/cwiid/libcwiid')
import cwiid
import time

def init():
	print 'Put Wiimote in discoverable mode now (press 1+2)...'
	global wiimote
	global named_calibration 
	if len(sys.argv) > 1:
		wiimote = cwiid.Wiimote(sys.argv[1])
	else:
		wiimote = cwiid.Wiimote()
	wiimote.rpt_mode = cwiid.RPT_BALANCE | cwiid.RPT_BTN
	wiimote.request_status()
	balance_calibration = wiimote.get_balance_cal()
	named_calibration = { 'right_top': balance_calibration[0],
						  'right_bottom': balance_calibration[1],
						  'left_top': balance_calibration[2],
						  'left_bottom': balance_calibration[3],
						}																						                    

def getweight():
	wiimote.request_status()
	readss = wiimote.state['balance']
	wiibalance_raw = readss.values()
	time.sleep(0.01)
	wiibalance_weight = calcweight(readss, named_calibration)
	return wiibalance_weight, wiibalance_raw

def calcweight( readings, calibrations):
	weight = [0,0,0,0]
	i = 0
	for sensor in ('left_bottom', 'right_top', 'right_bottom', 'left_top'):
		reading = readings[sensor]
		calibration = calibrations[sensor]
		if reading < calibration[1]:
			weight[i] = (1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]))/100.0
		else:
			weight[i] = (1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700)/100.0
		i+=1
	return weight


